/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <gtest/gtest.h>
#include <mrpt/core/bits_math.h>  // DEG2RAD
#include <mrpt/math/CAtan2LookUpTable.h>
#include <mrpt/random.h>

#include <cmath>

template <class LUT_CLASS>
void atan2_lut_test(
    const LUT_CLASS& atan2lut,
    const double SIZE,
    const double max_deg_errors,
    const double skip_area)
{
  for (int i = 0; i < 1000; i++)
  {
    const double x = mrpt::random::getRandomGenerator().drawUniform(-.5 * SIZE, .5 * SIZE);
    const double y = mrpt::random::getRandomGenerator().drawUniform(-.5 * SIZE, .5 * SIZE);

    // Avoid the central part, where accuracy is worse
    if (std::abs(x) < skip_area || std::abs(y) < skip_area) continue;

    const double atan2_good = ::atan2(y, x);
    double atan2_lut = .0;
    bool atan2_lut_valid = atan2lut.atan2(y, x, atan2_lut);

    EXPECT_TRUE(atan2_lut_valid);
    EXPECT_LT(std::abs(atan2_good - atan2_lut), mrpt::DEG2RAD(max_deg_errors))
        << "(x,y): (" << x << " , " << y << ")"
        << "\natan2_good:" << atan2_good << " atan2_lut:" << atan2_lut << std::endl;
  }
}

TEST(CAtan2LookUpTable, ValidValidTest)
{
  const double SIZE = 20.0;
  const double RES = 0.10;
  mrpt::math::CAtan2LookUpTable atan2lut(-.5 * SIZE, .5 * SIZE, -.5 * SIZE, .5 * SIZE, RES);
  atan2_lut_test(atan2lut, SIZE, 1.5 /*max error*/, 2.0 /* skip zone */);
}

TEST(CAtan2LookUpTable, MultiResTest)
{
  const double SIZE = 20.0;
  mrpt::math::CAtan2LookUpTableMultiRes atan2lut;

  std::map<double, double> res2extension;
  res2extension[0.001] = 0.8;  // 0.1 cm resolution
  res2extension[0.01] = 2.0;   // 1.0 cm resolution
  res2extension[0.02] = 5.0;   // 2.0 cm resolution
  res2extension[0.05] = 11.0;  // 5.0 cm resolution
  atan2lut.resize(res2extension);

  atan2_lut_test(atan2lut, SIZE, 0.5 /*max error*/, 0.4 /* skip zone */);
}
